# Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) (template)
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
#     http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
#
#
# Author: Dr. Denis
#

from launch import LaunchDescription
from launch.actions import IncludeLaunchDescription,DeclareLaunchArgument, RegisterEventHandler, TimerAction
from launch.event_handlers import OnProcessExit, OnProcessStart
from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution,PythonExpression
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare
from ament_index_python.packages import get_package_share_directory
from launch.launch_description_sources import PythonLaunchDescriptionSource
# from launch.substitutions import LaunchConfiguration, PythonExpression
from launch.conditions import IfCondition,UnlessCondition

def generate_launch_description():
    namespace  = "/robot"

    #声明参数
    declared_arguments = []
    declared_arguments.append(
        DeclareLaunchArgument(
            "runtime_config_package",
            default_value="agv_bringup",
            description='Package with the controller\'s configuration in "config" folder. \
        Usually the argument is not set, it enables use of a custom setup.',
        )
    )
    declared_arguments.append(
        DeclareLaunchArgument(
            "controllers_file",
            default_value="agv_controllers.yaml",
            description="YAML file with the controllers configuration.",
        )
    )
    declared_arguments.append(
        DeclareLaunchArgument(
            "description_package",
            default_value="agv_description",
            description="Description package with robot URDF/xacro files. Usually the argument \
        is not set, it enables use of a custom description.",
        )
    )
    declared_arguments.append(
        DeclareLaunchArgument(
            "description_file",
            default_value="agv.urdf.xacro",
            description="URDF/XACRO description file with the robot.",
        )
    )
    declared_arguments.append(
        DeclareLaunchArgument(
            "prefix",
            default_value='""',
            description="Prefix of the joint names, useful for \
        multi-robot setup. If changed than also joint names in the controllers' configuration \
        have to be updated.",
        )
    )
    declared_arguments.append(
        DeclareLaunchArgument(
            "use_mock_hardware",
            default_value="false",
            description="Start robot with mock hardware mirroring command to its states.",
        )
    )
    declared_arguments.append(
        DeclareLaunchArgument(
            "mock_sensor_commands",
            default_value="false",
            description="Enable mock command interfaces for sensors used for simple simulations. \
            Used only if 'use_mock_hardware' parameter is true.",
        )
    )
    # declared_arguments.append(
    #     DeclareLaunchArgument(
    #         "robot_controller",
    #         default_value="forward_position_controller",
    #         choices=["forward_position_controller", "joint_trajectory_controller"],
    #         description="Robot controller to start.",
    #     )
    # )
    declared_arguments.append(
        DeclareLaunchArgument(
            "sim_gazebo",
            default_value="false",
            description="",
        )
    )
    
    # Initialize Arguments
    runtime_config_package = LaunchConfiguration("runtime_config_package")
    controllers_file = LaunchConfiguration("controllers_file")
    description_package = LaunchConfiguration("description_package")
    description_file = LaunchConfiguration("description_file")
    prefix = LaunchConfiguration("prefix")
    use_mock_hardware = LaunchConfiguration("use_mock_hardware")
    mock_sensor_commands = LaunchConfiguration("mock_sensor_commands")
    # robot_controller = LaunchConfiguration("robot_controller")
    sim_gazebo = LaunchConfiguration("sim_gazebo")
    
    robot_controllers = PathJoinSubstitution(
        [FindPackageShare(runtime_config_package), "config", controllers_file]
    )
    # Get URDF via xacro
    robot_description_content = Command(
        [
            PathJoinSubstitution([FindExecutable(name="xacro")]),
            " ",
            PathJoinSubstitution(
                [FindPackageShare(description_package), "urdf", description_file]
            ),
            " ",
            "prefix:=",
            prefix,
            " ",
            "sim_gazebo:=",
            sim_gazebo,
            " ",
            "use_mock_hardware:=",
            use_mock_hardware,
            " ",
            "mock_sensor_commands:=",
            mock_sensor_commands,
            " ",
            "simulation_controllers:=",
            robot_controllers,
            " ",
        ]
    )

    robot_description = {"robot_description": robot_description_content}

    # robot_controllers = PathJoinSubstitution(
    #     [FindPackageShare(runtime_config_package), "config", controllers_file]
    # )
    
    rviz_config_file = PathJoinSubstitution(
        [FindPackageShare(description_package), "rviz", "agv.rviz"]
    )
    pkg_ignition_bringup = get_package_share_directory(
        'agv_gazebo')
    ign_launch = PathJoinSubstitution(
        [pkg_ignition_bringup, 'launch', 'ignition.launch.py'])

    control_node = Node(
        package="controller_manager",
        executable="ros2_control_node",
        output="both",
        namespace=namespace,
        # parameters=[ robot_controllers],
        arguments=["--ros-args", "--log-level", "INFO"],
        parameters=[robot_description, robot_controllers],
        condition=UnlessCondition(sim_gazebo)
    )

    gazebo_node = IncludeLaunchDescription(
        PythonLaunchDescriptionSource([ign_launch]),
        condition=IfCondition(sim_gazebo)
    )
        
    robot_state_pub_node = Node(
        package="robot_state_publisher",
        executable="robot_state_publisher",
        output="both",
        namespace=namespace,
        parameters=[robot_description],
    )
        
    delay_robot_state_pub_node = TimerAction(
        period=2.0,
        actions=[robot_state_pub_node]
    )
        
    rviz_node = Node(
        package="rviz2",
        executable="rviz2",
        name="rviz2",
        output="log",
        arguments=["-d", rviz_config_file],
    )

    joint_state_broadcaster_spawner = Node(
        package="controller_manager",
        namespace=namespace,
        executable="spawner",
        arguments=["joint_state_broadcaster", "--controller-manager", namespace+"/controller_manager","--controller-manager-timeout","10000" ,'--ros-args', '--log-level', 'INFO'],
    )
    # robot_controller_names = ["forward_position_controller","forward_velocity_controller","servo_velocity_controller","agv_controller","four_wheel_steering_controller"]
    # robot_controller_names = ["forward_position_controller","forward_velocity_controller","four_wheel_steering_controller","agv_controller"]
    robot_controller_names = ["agv_controller"]
    # robot_controller_names = ["four_wheel_steering_controller"]
    robot_controller_spawners = []
    for controller in robot_controller_names:
        robot_controller_spawners += [
            Node(
                package="controller_manager",
                namespace=namespace,
                executable="spawner",
                arguments=[controller, "-c", namespace+"/controller_manager","--controller-manager-timeout","10000" ,'--ros-args', '--log-level', 'INFO'],
            )
        ]

    inactive_robot_controller_names = ["add_some_controller_name"]
    inactive_robot_controller_spawners = []
    for controller in inactive_robot_controller_names:
        inactive_robot_controller_spawners += [
            Node(
                package="controller_manager",
                namespace=namespace,
                executable="spawner",
                arguments=[controller, "-c", namespace+"/controller_manager", "--inactive","--controller-manager-timeout","10000" ,'--ros-args', '--log-level', 'INFO'],
            )
        ]
    # if IfCondition(sim_gazebo):
        # Delay loading and activation of `joint_state_broadcaster` after start of ros2_control_node
    delay_joint_state_broadcaster_spawner_after_ros2_control_node = RegisterEventHandler(
        event_handler=OnProcessStart(
            target_action=control_node,
            on_start=[
                TimerAction(
                    period=3.0,
                    actions=[joint_state_broadcaster_spawner],
                ),
            ],
        )
    )
    
    delay_joint_state_broadcaster_spawner = TimerAction(
        period=5.0,
        actions=[joint_state_broadcaster_spawner]
    )

    delay_robot_controller_spawners_after_joint_state_broadcaster_spawner = []
    for i, controller in enumerate(robot_controller_spawners):
        delay_robot_controller_spawners_after_joint_state_broadcaster_spawner += [
            RegisterEventHandler(
                event_handler=OnProcessExit(
                    target_action=robot_controller_spawners[i - 1]
                    if i > 0
                    else joint_state_broadcaster_spawner,
                    on_exit=[controller],
                )
            )
        ]

    pose2vel_node = Node(
        package="pose2vel",
        executable="pose2vel_node",
        namespace=namespace,
        output="both",
        # arguments=['--ros-args', '--log-level', 'DEBUG'],
    )    
    diagnastic_node = Node(
        package="agv_diagnostics",
        executable="agv_diagnostics_node",
        namespace=namespace,
        output="both",
        # arguments=['--ros-args', '--log-level', 'DEBUG'],
    )
    
    delay_inactive_robot_controller_spawners_after_joint_state_broadcaster_spawner = []
    for i, controller in enumerate(inactive_robot_controller_spawners):
        delay_inactive_robot_controller_spawners_after_joint_state_broadcaster_spawner += [
            RegisterEventHandler(
                event_handler=OnProcessExit(
                    target_action=inactive_robot_controller_spawners[i - 1]
                    if i > 0
                    else robot_controller_spawners[-1],
                    on_exit=[controller],
                )
            )
        ]
        
    imu901_node = Node(
        package="imu901_reader",
        executable="imu_reader_node",
        namespace=namespace,
        output="both",
        arguments=['--ros-args', '-p', 'device_name:=/dev/ttyUSB0', '-p', 'baud_rate:=115200', '--log-level', 'INFO'],
        # arguments=['--ros-args', '--log-level', 'DEBUG'],
    )    
    litra_node1 = Node(
        package="ltme_node", executable="ltme_node", 
    #   node_name=["ltme_node"],
        output="screen",
        parameters=[
            { "topic_name": "scan1" },
            { "device_model": "LTME-02A" },
            { "device_address": "192.168.10.160" },
             { "average_factor": 8 },
        ]
    )
    litra_node2 = Node(
        package="ltme_node", executable="ltme_node", 
    #   node_name=["ltme_node"],
        output="screen",
        parameters=[
            { "topic_name": "scan2" },
            { "device_model": "LTME-02A" },
            { "device_address": "192.168.10.160" },
             { "average_factor": 8 },
        ]
    )
    litra_node3= Node(
        package="ltme_node", executable="ltme_node", 
    #   node_name=["ltme_node"],
        output="screen",
        parameters=[
            { "topic_name": "scan3" },
            { "device_model": "LTME-02A" },
            { "device_address": "192.168.10.160" },
             { "average_factor": 8 },
        ]
    )
    litra_node4 = Node(
        package="ltme_node", executable="ltme_node", 
    #   node_name=["ltme_node"],
        output="screen",
        parameters=[
            { "topic_name": "scan4" },
            { "device_model": "LTME-02A" },
            { "device_address": "192.168.10.160" },
             { "average_factor": 8 },
        ]
    )
    litra_node5 = Node(
        package="ltme_node", executable="ltme_node", 
    #   node_name=["ltme_node"],
        output="screen",
        parameters=[
            { "topic_name": "scan5" },
            { "device_model": "LTME-02A" },
            { "device_address": "192.168.10.160" },
             { "average_factor": 8 },
        ]
    )
    ld = LaunchDescription(declared_arguments)
    
    return LaunchDescription(
        declared_arguments
        + [
            litra_node,
            control_node,
            gazebo_node,
            pose2vel_node,
            delay_robot_state_pub_node,
            delay_joint_state_broadcaster_spawner,
        ]
        + delay_robot_controller_spawners_after_joint_state_broadcaster_spawner
    )
